/**
  ******************************************************************************
  * @file   
  * @brief  
  ******************************************************************************
  * @note

 */


/* Includes ------------------------------------------------------------------*/
#include "Service_Algorithm.h"
/* Private variables ---------------------------------------------------------*/

/*example*/
integrator_op integrator;
PID_OP PID_1;
/*********/

/* Global variables ---------------------------------------------------------*/
TaskHandle_t Alogrithm_Handle;

/*Motor_Control Example*/
PID_OP PID_Motor_Pos;
PID_OP PID_Motor_Speed;
/*********/

/* Definitions ---------------------------------------------------------*/
void Task_Alogrithm(void *arg);
/*Founctions------------------------------------------------------------------*/
void Service_Alogrithm_Init(void)
{
		/*example*/
  	systick_integrator_init(&integrator,1,-1);
		PID_init(&PID_1,0.02,10,1,2,0.01,10,-10,10,-10);
	
		PID_init(&PID_Motor_Pos,0.02,10,2,0,1,5,-5,5,-5);
		PID_init(&PID_Motor_Speed,0.02,10,0.5,0.1,0,3,-3,3,-3);
		/*********/
	
    xTaskCreate(Task_Alogrithm,          /* Task function. */
                "Alogrithm_Service",     /* Task Name. */
                Huge_Stack_Size,   /* Stack depth. */
                NULL,                /* Task parameter */
                PriorityNormal, /* Priority */
                &Alogrithm_Handle);      /* Task handle */
}

void Task_Alogrithm(void *arg)
{
    for (;;)
    {		
				/*example*/
				PID_control(&PID_1,0.01);
				systick_integrator(-0.5,&integrator);
				/*********/
			
				/*Motor_Control Example*/
				PID_control(&PID_Motor_Pos,300-Motor1.Angle);
				PID_control(&PID_Motor_Speed,PID_Motor_Pos.output_value-Motor1.Speed);		
				/*********/
			
        vTaskDelay(1);  // Pass to next ready task
    }
}

/* Exported function declarations --------------------------------------------*/


